#pragma comment(lib, "libARd") 
#pragma comment(lib, "libARgsub_lited")
#pragma comment(lib, "libARgsubd")
#pragma comment(lib, "libARvideod")

#include <AR\ar.h>
#include <AR\video.h>
#include <AR\param.h>
#include <AR\matrix.h>
#include <AR\config.h>
#include <AR\gsub_lite.h>
#include <AR\gsub.h>
#include <NuiApi.h>

#include "debug_aux.h"

#include "markerInfo.h"

#define AR_DATA ".\\ARparam\\"
#define AR_CAMERA_PARAM ".\\ARparam\\camera_para.dat"
#define AR_PATTERN1 ".\\ARparam\\a.kpat"
#define AR_PATTERN2 ".\\ARparam\\b.kpat"
#define AR_PATTERN3 ".\\ARparam\\c.kpat"
#define AR_PATTERN4 ".\\ARparam\\d.kpat"

// initialization flag
bool Started = false;
unsigned char ImageTreshold = 100;

// initialization camera parameters
ARParam CameraParameters;

// current frame marker information
ARMarkerInfo * MarkerInfo;

// array containing array positions of the found markers for current frame,
// in the MarkerInfo array, being position 0 first marker, position 1 second 
// marker, and so on, if ID < 0, then the marker was not found
int markerFoundIndex[4];

// when loaded, patterns will receive ids stored
// in this array
int PatternId[4];

double MarkerSize = 10;

bool MarkerInfo_init(double markerSizeInMm) {
	ARParam param;
	if (!Started) {
		MarkerSize = markerSizeInMm;
		if (arParamLoad(AR_CAMERA_PARAM, 1, &param) < 0) {
			return false;
		}
		PatternId[0] = arLoadPatt(AR_PATTERN1);
		PatternId[1] = arLoadPatt(AR_PATTERN2);
		PatternId[2] = arLoadPatt(AR_PATTERN3);
		PatternId[3] = arLoadPatt(AR_PATTERN4);
		for (int i = 0; i < 4; i++) {
			if (PatternId[i] < 0) {
				return false;
			}
		}

		arParamChangeSize(&param, 640, 480, &CameraParameters);

		CameraParameters.mat[0][0] = NUI_CAMERA_COLOR_NOMINAL_FOCAL_LENGTH_IN_PIXELS;
		CameraParameters.mat[1][1] = NUI_CAMERA_COLOR_NOMINAL_FOCAL_LENGTH_IN_PIXELS;
		CameraParameters.mat[2][2] = 1.0;
		CameraParameters.mat[0][2] = 320;
		CameraParameters.mat[1][2] = 240;

		arInitCparam(&CameraParameters);

		Started = true;
	}
	return true;
}

void MarkerInfo_parseImageFrame(unsigned long * imageFrame) {
	int markerNum;

	if (Started) {
		if (arDetectMarker((ARUint8 *)imageFrame, ImageTreshold, &MarkerInfo, &markerNum) < 0) {
			return;
		}
		for (int i = 0; i < 4; i++) {					// for each pattern id, find the
			markerFoundIndex[i] = -1;					// best matching marker, if any
			for (int j = 0; j < markerNum; j++) {
				if (MarkerInfo[j].id == i) {			// if pattern is found at least once,
					if (markerFoundIndex[i] < 0) {		// it is for now the best matching
						markerFoundIndex[i] = j;
					} else if (MarkerInfo[j].cf > MarkerInfo[markerFoundIndex[i]].cf) {
						markerFoundIndex[i] = j;		// a better match was found
					}
				}
			}
		}
	}
}

bool MarkerInfo_getMarkerMatrix(int markerId, double matrix[3][4]) {
	if (!Started || markerFoundIndex[markerId] < 0) {
		return false;
	} else {
		double center[2] = {0, 0};
		double tmp[3][4];
		double inv[3][4] =  {
			 1,  0,  0,  0,
			 0, -1,  0,  0,
			 0,  0, -1,  0
		};
		arGetTransMat(&MarkerInfo[markerFoundIndex[markerId]], center, MarkerSize, tmp);
		// convert to opengl axis orientation
		MarkerInfo_matrixMultiply(matrix, inv, tmp);
		return true;
	}
}

void MarkerInfo_convertArToGlMatrix(double arMatrix[3][4], double glMatrix[16]) {
	argConvGlpara(arMatrix, glMatrix);
}

void MarkerInfo_convertGlToArMatrix(double glMatrix[16], double arMatrix[3][4]) {
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 4; j++) {
			arMatrix[i][j] = glMatrix[i + j * 4];
		}
	}
}

ARMat MarkerInfo_createARMat(double arMatrix[3][4]) {
	ARMat ret;
	
	ret.m = (double *)malloc(sizeof(double) * 16);
	ret.clm = 4;
	ret.row = 4;
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 4; j++) {
			ret.m[i * 4 + j] = arMatrix[i][j];
		}
	}
	ret.m[12] = ret.m[13] = ret.m[14] = 0.0;
	ret.m[15] = 1.0;
	return ret;
}

ARMat MarkerInfo_createARMat(double glMatrix[16]) {
	double arMatrix[3][4];
	MarkerInfo_convertGlToArMatrix(glMatrix, arMatrix);
	return MarkerInfo_createARMat(arMatrix);
}

void MarkerInfo_deleteARMat(ARMat * m) {
	free(m->m);
}

void MarkerInfo_matrixMultiply(double glMatrixResult[16], double glMatrixA[16], double glMatrixB[16]) {
	double arMatrixResult[3][4], arMatrixA[3][4], arMatrixB[3][4];

	MarkerInfo_convertGlToArMatrix(glMatrixA, arMatrixA);
	MarkerInfo_convertGlToArMatrix(glMatrixB, arMatrixB);
	MarkerInfo_matrixMultiply(arMatrixResult, arMatrixA, arMatrixB);
	MarkerInfo_convertArToGlMatrix(arMatrixResult, glMatrixResult);
}

void MarkerInfo_matrixMultiply(double arMatrixResult[3][4], double arMatrixA[3][4], double arMatrixB[3][4]) {
	arUtilMatMul(arMatrixA, arMatrixB, arMatrixResult);
}

void MarkerInfo_matrixInvert(double arMatrixOriginal[3][4], double arMatrixInverted[3][4]) {
	arUtilMatInv(arMatrixOriginal, arMatrixInverted);
}

void MarkerInfo_matrixInvert(double glmatrixOriginal[16], double glMatrixInverted[16]) {
	double arMatrixOriginal[3][4], arMatrixInverted[3][4];

	MarkerInfo_convertGlToArMatrix(glmatrixOriginal, arMatrixOriginal);
	MarkerInfo_matrixInvert(arMatrixOriginal, arMatrixInverted);
	MarkerInfo_convertArToGlMatrix(arMatrixInverted, glMatrixInverted);
}

void MarkerInfo_getProjectionMatrix(double glMatrix[16]) {
	arglCameraFrustum(&CameraParameters, 3.0, 10000.0, glMatrix);
}

void MarkerInfo_setTreshold(unsigned char treshold) {
	ImageTreshold = treshold;
}
unsigned char MarkerInfo_getTreshold() {
	return ImageTreshold;
}